//
// Created by ze on 2023/11/9.
//

#include "UPRE_PID.h"
#include "UPRE_LOCATION.h"
#include "UPRE_ROBOT_TASK.h"
#include "Kalmanfilter.h"
Pose Print_Target_Pose = {0};
static PidCalMode calMode = PID_OFF;
static PoseRegulator poseRegulator = { 0 };
void Init_PID_uint(PIDInfo* p)
{
    p->ki = (p->kp * p->T) / p->ti;
    p->kd = (p->kp * p->td) / p->T;
    p->ka = p->kp + ((p->kp) * p->T) / p->ti + p->kp * (p->td / p->T);
    p->kb = p->kp + p->kp * 2 * (p->td / p->T);
    p->kc = p->kp * (p->td / p->T);
}
void pid_param_init(
        PIDInfo* pid,
        float maxout,
        float limit_integral,
        float U_n,
        float kp,
        float ti,
        float td,
        float T,
        bool M)
{
    pid->limit_Integral = limit_integral;
    pid->MaxOutput = maxout;
    pid->Un = U_n;
    pid->kp = kp;
    pid->ti = ti;
    pid->td = td;
    pid->T = T;
    pid->output = 0;
    pid->pid_MODE = M;
    Init_PID_uint(pid);
}




static void  pidClear(PIDInfo* pid)
{
    pid->last_err = 0;
    pid->output = 0;
}
/***********************************************************
 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零
 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零
 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零 * ti 莫要设零
 * ********************************************************/
void pidInit(void)
{

    ////自身角度+
    pid_param_init(&poseRegulator.yawRegulator,
                   1000.f,
                   100.f,
                   0.f,
                   1.5f,
                   100000.f,
                   0.f,
                   1.f,
                   0);
    ////X方向平移
    pid_param_init(&poseRegulator.xRegulator,
                   1500.f,
                   100.f,
                   0.f,
                   8.f,
                   100000.f,
                   0.f,
                   1.f,
                   0);
    ////y方向平移
    pid_param_init(&poseRegulator.yRegulator,
                   1500.f,
                   100.f,
                   0.f,
                   8.f,
                   100000.f,
                   0.f,
                   1.f,
                   0);
}
void setPidCalMode(PidCalMode mode)
{
    calMode = mode;
    poseRegulator.xRegulator.target = Robot_Pose.wx;
    poseRegulator.yRegulator.target = Robot_Pose.wy;
    poseRegulator.yawRegulator.target = Robot_Pose.yaw;

    switch (mode)
    {
        case PID_OFF:
            pidClear(&poseRegulator.xRegulator);
            pidClear(&poseRegulator.yRegulator);
            pidClear(&poseRegulator.yawRegulator);
            break;
        case PID_YAW_ONLY:
            pidClear(&poseRegulator.yawRegulator);
            break;
        case PID_X_Y_ONLY:
            pidClear(&poseRegulator.xRegulator);
            pidClear(&poseRegulator.yRegulator);
            break;
        case PID_X_Y_YAW:

            pidClear(&poseRegulator.xRegulator);
            pidClear(&poseRegulator.yRegulator);
            pidClear(&poseRegulator.yawRegulator);
            break;
        default:
            break;
    }
}
void setPidTargetPose(Pose* targetPose, PidCalMode calMode)
{
//    printf("target:%f\r\n",targetPose->yaw);
    Print_Target_Pose = *targetPose;
    targetPose->yaw = Limit_MaxAndMin(targetPose->yaw,100.f,-100.f);
    switch (calMode) {
        case PID_OFF:
            break;
        case PID_YAW_ONLY:
            if(fabsf(targetPose->yaw - Robot_Pose.yaw) >181.f)      //180To-180陀螺仪专用
                targetPose->yaw = -targetPose->yaw;
            poseRegulator.yawRegulator.target = targetPose->yaw;
            break;
        case PID_X_Y_ONLY:
            poseRegulator.xRegulator.target = targetPose->wx;
            poseRegulator.yRegulator.target = targetPose->wy;
            break;
        case PID_X_Y_YAW:

            poseRegulator.xRegulator.target = targetPose->wx;
            poseRegulator.yRegulator.target = targetPose->wy;
            poseRegulator.yawRegulator.target = targetPose->yaw;
            break;
    }
}
/**
 * 通用PID
 * */
float pid_calculate(PIDInfo* pid, float measure)
{
    static float err, U_k = 0;
    err = pid->target - measure;
    if (err > (pid->Un) || (err < -(pid->Un))) {

        if (pid->pid_MODE)
        {

            U_k = pid->last_output + pid->ka * err - pid->kb * pid->last_err + pid->kc * pid->lalast_err;
        }
        else
        {
            if (pid->Integral > pid->limit_Integral) { pid->Integral = pid->limit_Integral; }
            if (pid->Integral < -pid->limit_Integral) { pid->Integral = -pid->limit_Integral; }
            U_k = pid->kp * err + pid->ki * pid->Integral + pid->kd * (err - pid->last_err);
            pid->Integral += err;
        }
    }

    pid->last_output = U_k;
    pid->lalast_err = pid->last_err;
    pid->last_err = err;
    if (U_k > pid->MaxOutput)
        U_k = pid->MaxOutput;
    if (U_k < -pid->MaxOutput)
        U_k = -pid->MaxOutput;
    pid->output = U_k;
    return pid->output;
}
//void ctrlEntry()
//{
//
//    switch (calMode) {
//        case PID_OFF: {
//            pidClear(&poseRegulator.xRegulator);
//            pidClear(&poseRegulator.yRegulator);
//            pidClear(&poseRegulator.yawRegulator);
//            break;
//        }
//        case PID_YAW_ONLY: {
//            pid_calculate(&poseRegulator.yawRegulator, Robot_Pose.yaw);
//            break;
//        }
//        case PID_X_Y_ONLY: {
//            pid_calculate(&poseRegulator.xRegulator, Robot_Pose.wx);
//            pid_calculate(&poseRegulator.yRegulator, Robot_Pose.wy);
//            break;
//        }
//        case PID_X_Y_YAW: {
//            pid_calculate(&poseRegulator.xRegulator, Robot_Pose.wx);
//            pid_calculate(&poseRegulator.yRegulator, Robot_Pose.wy);
//            pid_calculate(&poseRegulator.yawRegulator, Robot_Pose.yaw);
//
//            break;
//        }
//        default:
//            break;
//    }
//}
ChassisMovement getPidOutput()
{
    ChassisMovement poseAdjust = { 0 };
    switch (calMode)
    {
        case PID_OFF: {
            poseAdjust.transVel.size = 0.f;
            poseAdjust.omega = 0.f;
            pidClear(&poseRegulator.xRegulator);
            pidClear(&poseRegulator.yRegulator);
            pidClear(&poseRegulator.yawRegulator);
            break;
        }
        case PID_YAW_ONLY: {
            poseRegulator.yawRegulator.output = pid_calculate(&poseRegulator.yawRegulator, Robot_Pose.yaw);
            poseAdjust.omega = poseRegulator.yawRegulator.output;
            poseAdjust.transVel.size = 0;

            break;
        }
        case PID_X_Y_ONLY: {
            poseRegulator.xRegulator.output = pid_calculate(&poseRegulator.xRegulator, Robot_Pose.wx);
            poseRegulator.yRegulator.output = pid_calculate(&poseRegulator.yRegulator, Robot_Pose.wy);
            poseAdjust.transVel.dir = Radian2Angle(atan2f(poseRegulator.yRegulator.output, poseRegulator.xRegulator.output));
            poseAdjust.transVel.size = sqrtf(square(poseRegulator.xRegulator.output) + square(poseRegulator.yRegulator.output));
            poseAdjust.omega = 0.f;
            break;
        }
        case PID_X_Y_YAW: {
            poseRegulator.xRegulator.output = pid_calculate(&poseRegulator.xRegulator, Robot_Pose.wx);
            poseRegulator.yRegulator.output = pid_calculate(&poseRegulator.yRegulator, Robot_Pose.wy);
            poseRegulator.yawRegulator.output = pid_calculate(&poseRegulator.yawRegulator, Robot_Pose.yaw);
            poseAdjust.transVel.dir = Radian2Angle(atan2f(poseRegulator.yRegulator.output, poseRegulator.xRegulator.output));
            poseAdjust.transVel.size = sqrtf(square(poseRegulator.xRegulator.output) + square(poseRegulator.yRegulator.output));
            poseAdjust.omega = poseRegulator.yawRegulator.output;
            break;
        }

    }

    return poseAdjust;
}


